/**
 * @file rc_usart.cpp
 * @author jozenlee (2250017028@qq.com)
 * @brief
 * @version 0.1
 * @date 2022-03-04
 *
 * @copyright Copyright (c) 2022
 *
 */
#include "common/Driver/rc_usart.h"
#include <math.h>

/**
 * @brief Convert angle data from float into uint16_t according to HT protocol
 *
 */
uint16_t RC_Usart::float_to_uint(float x, float x_min, float x_max, uint8_t bits) {
    float span = x_max - x_min;
    float offset = x_min;
    LIMIT_MIN_MAX(x, x_min, x_max);
    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) /
                      span);    //转换为一个16进制数,其中0代表最小数，65535代表最大数
}

float RC_Usart::uint_to_float(uint16_t x, float x_min, float x_max, uint8_t bits) {
    float span = x_max - x_min;
    float offset = x_min;
    return (float)x * span / ((float)((1 << bits) - 1)) + offset;
}

/**
 * @brief Update data from robot
 *
 */
void RC_Usart::updateData(LegData* data) {
    uint8_t rev[sizeof(UsartDataType)];

    // update date before get data
    if (!usart1.updateData()) {
        // get data of front legs
        usart1.getData(rev, sizeof(UsartDataType));
        if(!Unpack_LegData(rev, data)){
            stm32InitFlag[0] = true;
        }
        
    }

    if (!usart2.updateData()) {
        // get data of hind legs
        usart2.getData(rev, sizeof(UsartDataType));
        if(!Unpack_LegData(rev, data))
        stm32InitFlag[1]=true;
    }

    
}

/**
 * @brief Update command to control robot
 *
 * @param src
 * @param size
 */
void RC_Usart::updateCommand(const RobotCommand* src) {
    uint8_t trans[2][sizeof(UsartCmdType)];

    // std::cout<<src->q_des_hip[0]<<std::endl;

    Pack_RobotCmd(src, trans[0], trans[1]);
    usart1.sendData(trans[0], sizeof(UsartCmdType));
    usart2.sendData(trans[1], sizeof(UsartCmdType));

    // test
    // UsartCmdType cmd;
    // Unpack_RobotCmd(1, trans[0], &cmd);
    // std::cout<<cmd.qd_des_abad[0]<<" "<<cmd.qd_des_hip[0]<<" "<<cmd.qd_des_knee[0]<<std::endl;
}

/**
 * @brief  Pack robot command into uint8 array
 *
 * @param src
 * @param dst
 */
void RC_Usart::Pack_RobotCmd(const RobotCommand* src, uint8_t* dst1, uint8_t* dst2) {
    UsartCmdType cmd[2];
    cmd[0].head = 1;
    cmd[1].head = au_cmd;// cmd[1].head = 2;
    cmd[0].tail = VERIFY_VALUE;
    cmd[1].tail = VERIFY_VALUE;

    // data copy and convert
    uint8_t cmd_i;
    for (int leg = 0; leg < 4; leg++) {
        if (leg == 0 || leg == 1) {
            cmd_i = 0;
        } else
            cmd_i = 1;

        cmd[cmd_i].q_des_abad[leg - 2 * cmd_i] = float_to_uint((!cmd_i) ? -src->q_des_abad[leg] : src->q_des_abad[leg], P_MIN, P_MAX, 16);
        cmd[cmd_i].q_des_hip[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3) ? 
                                                                                                                            -(M_PI_2 - src->q_des_hip[leg]) : M_PI_2 - src->q_des_hip[leg], P_MIN, P_MAX, 16);
        cmd[cmd_i].q_des_knee[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3) ? 
                                                                                                                            -(M_PI_2 + src->q_des_knee[leg]) : M_PI_2 + src->q_des_knee[leg], P_MIN, P_MAX, 16);

        cmd[cmd_i].qd_des_abad[leg - 2 * cmd_i] = float_to_uint((!cmd_i) ? -src->qd_des_abad[leg] : src->qd_des_abad[leg], V_MIN, V_MAX, 12);
        cmd[cmd_i].qd_des_hip[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3)? 
                                                                                                                            src->qd_des_hip[leg] : -src->qd_des_hip[leg], V_MIN, V_MAX, 12);
        cmd[cmd_i].qd_des_knee[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3)? 
                                                                                                                            -src->qd_des_knee[leg] : src->qd_des_knee[leg], V_MIN, V_MAX, 12);

        cmd[cmd_i].kd_abad[leg - 2*cmd_i]= float_to_uint(src->kd_abad[leg], KD_MIN, KD_MAX, 12);
        cmd[cmd_i].kd_hip[leg - 2*cmd_i]= float_to_uint(src->kd_hip[leg], KD_MIN, KD_MAX, 12);
        cmd[cmd_i].kd_knee[leg - 2*cmd_i]= float_to_uint(src->kd_knee[leg], KD_MIN, KD_MAX, 12);

        cmd[cmd_i].kp_abad[leg - 2*cmd_i]= float_to_uint(src->kp_abad[leg], KP_MIN, KP_MAX, 12);
        cmd[cmd_i].kp_hip[leg - 2*cmd_i]= float_to_uint(src->kp_hip[leg], KP_MIN, KP_MAX, 12);
        cmd[cmd_i].kp_knee[leg - 2*cmd_i]= float_to_uint(src->kp_knee[leg], KP_MIN, KP_MAX, 12);

        cmd[cmd_i].tau_abad_ff[leg - 2 * cmd_i] = float_to_uint((leg == 0 || leg == 1) ? - src->tau_abad_ff[leg] : src->tau_abad_ff[leg],T_MIN, T_MAX, 12);
        cmd[cmd_i].tau_hip_ff[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3)? 
                                                                                                                            src->tau_hip_ff[leg] : -src->tau_hip_ff[leg], T_MIN, T_MAX, 12);
        cmd[cmd_i].tau_knee_ff[leg - 2 * cmd_i] = float_to_uint((leg == 1 || leg == 3)? 
                                                                                                                            -src->tau_knee_ff[leg] : src->tau_knee_ff[leg], T_MIN, T_MAX, 12);

        cmd[cmd_i].flags[leg - 2 * cmd_i] = src->flags[leg];
    }

    // pack data
    memcpy(dst1, &cmd[0], sizeof(UsartCmdType));
    memcpy(dst2, &cmd[1], sizeof(UsartCmdType));
}

/**
 * @brief Unpack robot data from uint8 array
 *
 * @param src
 * @param dst1
 * @param dst2
 */
uint8_t RC_Usart::Unpack_LegData(const uint8_t* src, LegData* dst) {
    UsartDataType data;
    uint8_t data_i;
    // unpack data
    memcpy(&data, src, sizeof(UsartDataType));
   //printf("head:%d,tail:%d\n",data.head,data.tail);
    // check data validity
    if (data.tail != VERIFY_VALUE)
        return 1;

    // check the source    1 RF&LF     2 RB&LB
    if (data.head == 1) {
        data_i = 0;
    } else if (data.head == 2) {
        data_i = 2;
    } else
        return 1; 

    // copy data
    for (int leg = 0; leg < 2; leg++) {
        dst->q_abad[leg + data_i] = (data.head == 1) ? -uint_to_float(data.q_abad[leg], P_MIN, P_MAX, 16) : uint_to_float(data.q_abad[leg], P_MIN, P_MAX, 16);

        dst->q_hip[leg + data_i] = uint_to_float(data.q_hip[leg], P_MIN, P_MAX, 16);
        if(leg) dst->q_hip[leg + data_i] = M_PI_2 + dst->q_hip[leg + data_i];
        else dst->q_hip[leg + data_i] = M_PI_2 - dst->q_hip[leg + data_i];

        dst->q_knee[leg + data_i] = uint_to_float(data.q_knee[leg] - M_PI_2, P_MIN, P_MAX, 16);
        if(leg) dst->q_knee[leg + data_i] = -dst->q_knee[leg + data_i] - M_PI_2;
        else dst->q_knee[leg + data_i] = dst->q_knee[leg + data_i] - M_PI_2;   

        dst->qd_abad[leg + data_i] =  (data.head == 1) ? -uint_to_float(data.qd_abad[leg], V_MIN, V_MAX, 12) : uint_to_float(data.qd_abad[leg], V_MIN, V_MAX, 12);
        
        dst->qd_hip[leg + data_i] = uint_to_float(data.qd_hip[leg], V_MIN, V_MAX, 12);
        if(!leg) dst->qd_hip[leg + data_i] = -dst->qd_hip[leg + data_i];

        dst->qd_knee[leg + data_i] = uint_to_float(data.qd_knee[leg], V_MIN, V_MAX, 12);
        if(leg) dst->qd_knee[leg + data_i] = -dst->qd_knee[leg + data_i];
        dst->flags[leg + data_i] = data.flags[leg];
    }

    return 0;
}

/**
 * @brief Unpack robot command from master controller
 *
 * @param src
 * @param dst
 */
void RC_Usart::Unpack_RobotCmd(uint8_t head, const uint8_t* src, UsartCmdType* dst) {
    // copy data
    memcpy(dst, src, sizeof(UsartCmdType));

    // check data validity
    if (head != dst->head || dst->tail != VERIFY_VALUE)
        dst = nullptr;
}

/**
 * @brief pack robot data to master controller
 *
 * @param src
 * @param dst
 */
void RC_Usart::Pack_LegData(const UsartDataType* src, uint8_t* dst) { memcpy(dst, src, sizeof(UsartDataType)); }